import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

def plot_f_degree_ani(input, angle, freq_motion, freq_angle):
    input = np.array(input)

    t_start = 0
    t_end = len(input)

    # 画一个周期
    f_min = calc_min_f(freq_motion)
    print("最小频率："+str(f_min)+"Hz")
    t_start = int(len(input)/2 - 1/f_min * freq_angle/2)
    t_end = int(len(input)/2 + 1/f_min * freq_angle/2)
    input = input[t_start:t_end,:]
    angle = angle[t_start:t_end,:]

    # print(t_start/freq_angle,t_end/freq_angle)

    pitch = angle[:,1]
    roll = angle[:,0]
    yaw = angle[:,2]

    headers = ["D", "L", "Y", "Mx", "My", "Mz", "Q"]

    fig, axs = plt.subplots(2, 3, figsize=(20, 10))

    def update(frame):
        for i in range(6):
            axs[i//3, i%3].clear()
            axs[i//3, i%3].set_title(headers[i])
            if(freq_motion[0]>0):
                axs[i//3, i%3].plot(pitch[:frame], input[:frame, i])
                axs[i//3, i%3].set_xlabel('Pitch')
            elif(freq_motion[1]>0):
                axs[i//3, i%3].plot(roll[:frame], input[:frame, i])
                axs[i//3, i%3].set_xlabel('Roll')
            elif(freq_motion[2]>0):
                axs[i//3, i%3].plot(yaw[:frame], input[:frame, i])
                axs[i//3, i%3].set_xlabel('Yaw')
            axs[i//3, i%3].set_ylabel('Degree')

    ani = FuncAnimation(fig, update, frames=len(input), interval=50)
    plt.show()
